#include "findCornerPoint.hpp"

int GetMeanThreshold(const Mat image)
{
    
    int Sum = 0;
    int Amount = 0;
    int idx = 0;
    double Histogram[256]={0}; //灰度直方图，下标是灰度值，保存内容是灰度值对应的像素点总数
    uchar *data=image.data;
    for(int i=0;i<image.rows;i++)   //为表述清晰，并没有把rows和cols单独提出来
    {
        for(int j=0;j<image.cols;j++)
        {
            Histogram[data[i*image.step+j]]++;//*
        }
    }
    for (int Y = 70; Y < 255; Y++)
    {
        //        Amount += Histogram[Y];
        //        Sum += Y * Histogram[Y];
        Amount +=Histogram[Y];
    }
    Amount = Amount / 2;
    for(int Y =70;Y<255;Y++)
    {
        if(Sum < Amount)
        {
            Sum += Histogram[Y];
            idx = Y;
        }
    }
    return idx;
}
bool deletePoint(vector<Point> &vecLinePoint, Mat srcImage)
{
    float slop1,slop2;
    std::vector<float> slop;
    int max = 0;
    float dist;
    dist = 0;
    for(int i = 0; i < vecLinePoint.size(); i++)
        for( int k = i+1; k < vecLinePoint.size() ; k++)
        {
            slop1 = (((float)vecLinePoint[i].y -(float) vecLinePoint[k].y )/((float) vecLinePoint[i].x- (float)vecLinePoint[k].x ));
            if(slop1 > 9999) slop1 =9999;
            slop.push_back(slop1);
            dist = dist +1;
        }
    slop2 = 0;
    for(int i=0; i < slop.size() - 1; ++i)
        if(max < count(slop.begin(), slop.end(), slop[i]))
        {
            max = count(slop.begin(), slop.end(), slop[i]);
            slop2 = slop[i];
            
        }
    int flag= 0;
    for(int i = 0; i < vecLinePoint.size() ; i++)
    {
        flag = 0;
        for(int k = 0; k < vecLinePoint.size() ; k++)
        {
            if(i != k)
            {
                slop1 = (((float)vecLinePoint[i].y -(float) vecLinePoint[k].y )/((float) vecLinePoint[i].x - (float)vecLinePoint[k].x ));
                if(slop1 > 9999) slop1 =9999;
                if(slop2== slop1)
                {
                    flag =1;
                }
            }
            
        }
        if( flag == 0)
        {
            vecLinePoint.erase(vecLinePoint.begin()+ i);
            i = i - 1;
            cout << "delete" << "  ";
        }
    }
    cout << endl;
    for(int  i = 0; i < vecLinePoint.size(); i++ )
        circle(srcImage,vecLinePoint[i],10,cv::Scalar( 128,255, 255 ),2,9,0);
    if(vecLinePoint.size() >=2)
        return true;
    else
        return false;
}

void LineFitLeastSquares(vector<cv::Point> &vecLinePoint, float &k, float &dist)
{
    
    float A = 0.0;
    float B = 0.0;
    float C = 0.0;
    float D = 0.0;
    
    for (int i = 0; i < vecLinePoint.size(); i++)
    {
        A +=(float) vecLinePoint[i].x *(float) vecLinePoint[i].x;
        B += (float)vecLinePoint[i].x;
        C +=(float) vecLinePoint[i].x *(float) vecLinePoint[i].y;
        D += (float)vecLinePoint[i].y;
    }
    
    // º∆À„–±¬ k∫ÕΩÿæ‡dist
    float  temp;
    temp=(vecLinePoint.size()*A - B*B);
    if( temp > 0.000001)// ≈–∂œ∑÷ƒ∏≤ªŒ™0
    {
        k = (vecLinePoint.size()*C - B*D) / temp;
        dist = (A*D - B*C) / temp;
    }
    else
    {
        k = 0;
        dist = B / vecLinePoint.size();
    }
    
    
}

cv::Point2f FindCornerPoint::computeIntersect(Line l1, Line l2)
{
    int x1 = l1._p1.x, x2 = l1._p2.x, y1 = l1._p1.y, y2 = l1._p2.y;
    int x3 = l2._p1.x, x4 = l2._p2.x, y3 = l2._p1.y, y4 = l2._p2.y;
    float k1,k2,b1,b2;
     cv::Point2f pt;
    k1=(y1 - y2) / (x1 - x2);
    b1= y1 - k1 * x1;
    if (x3 != x4) {
        k2=(y3 - y4) / (x3 - x4);
        b2= y3 - k2 * x3;
        pt.x = (b1 - b2) / ( k2 - k1);
    }
    else{
        pt.x = x3;
    }
    pt.y = k1 * pt.x + b1;
    return pt;

    
}


bool FindCornerPoint::getCornerInfo(cv::Mat srcImage, cv::Point &cornerPosition, float &turnAngle)
{
    Line VerticalLine,HorizontalLine;
    int Image_Width = srcImage.cols;
    int Image_Height = srcImage.rows;
    int thresh = GetMeanThreshold(srcImage);
    thresh *= 1.3;
    if(thresh<20)
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    cv::Mat srcImage1 = srcImage.clone();
    threshold(srcImage1, srcImage1, thresh, 255, THRESH_BINARY);
    
    int rectWidth = 10;
    int rectHeight = 10;
    cv::Rect rectHorizontal,rectVertical;
    rectHorizontal.width = rectWidth;rectHorizontal.height = rectHeight;
    rectVertical.width = rectWidth;rectVertical.height = rectHeight;
    int boxSum = 0;
    int thresh1 = rectHeight * rectWidth * 255 * 4000 / (thresh * thresh);
    int thresh2 = rectHeight * rectWidth * 100;
    cv::Point temp;
    std::vector<cv::Point> vecLinePoint,horLinePoint;
    
    //search horizontal line
    for(int idx = 5; idx < 15;++idx)
    {
        rectHorizontal.x = Image_Width/2 - idx * 15 ;
        for(int rowLoop = Image_Height/2 - 400 ; rowLoop<Image_Height/2 +900 ; ++rowLoop)
        {
            rectHorizontal.y = rowLoop;
            boxSum = cv::sum(srcImage1(rectHorizontal))[0];
      //     std::cout<<"x: "<<rectHorizontal.x<<" y: "<<rectHorizontal.y<<" boxSum: "<<boxSum<<std::endl;
            if(boxSum >thresh1)
            {
                temp.x = rectHorizontal.x;
                temp.y = rectHorizontal.y +10;
                break;
            }
        }
        circle(srcImage, temp, 10, Scalar::all(255),2,9,0);
        horLinePoint.push_back(temp);
    }
    if(horLinePoint.size() >= 2)
    {
        deletePoint(horLinePoint, srcImage);
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    Vec4f Hline,Vline;
    float slop = 0.0;
    float dist =0.0;
    if(horLinePoint.size() >= 2)
    {
        //LineFitLeastSquares( horLinePoint,slop,dist);
        cv::fitLine(horLinePoint, Hline, CV_DIST_HUBER, 0, 0.01, 0.01);
        HorizontalLine._p1.x = (Image_Width >> 1) -300;
        HorizontalLine._p1.y = (HorizontalLine._p1.x - Hline[2]) *Hline[1]/Hline[0] +Hline[3];
        HorizontalLine._p2.x = (Image_Width >> 1) +700;
        HorizontalLine._p2.y = (HorizontalLine._p2.x - Hline[2]) *Hline[1]/Hline[0] +Hline[3];
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    for(int idx = 1; idx < 21;++idx)
    {
        rectVertical.y = HorizontalLine._p1.y + idx * 20;
        for(int colLoop = Image_Width/2 - 400; colLoop < Image_Width/2 +300; ++colLoop)
        {
            rectVertical.x = colLoop;
            boxSum = cv::sum(srcImage1(rectVertical))[0];
            //std::cout<<"x: "<<rectVertical.x<<" y: "<<rectVertical.y<<" Sum: "<<boxSum<<std::endl;
            if(boxSum > thresh2)
            {
                temp.x = rectVertical.x + 10;
                temp.y = rectVertical.y;
                break;
            }
        }
        circle(srcImage, temp, 10, Scalar::all(255),2,9,0);
        vecLinePoint.push_back(temp);
    }
    if(vecLinePoint.size() >= 2)
    {
        deletePoint(vecLinePoint, srcImage);
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    if(vecLinePoint.size() >= 2)
    {
        cv::fitLine(vecLinePoint, Vline, CV_DIST_HUBER, 0, 0.01, 0.01);
        VerticalLine._p1.y = (Image_Height >> 1) - 800;
        VerticalLine._p1.x = (VerticalLine._p1.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
        VerticalLine._p2.y = (Image_Height >> 1) + 800;
        VerticalLine._p2.x = (VerticalLine._p2.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    cv::line(srcImage, cv::Point(VerticalLine._p1.x, VerticalLine._p1.y), cv::Point(VerticalLine._p2.x, VerticalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    cv::line(srcImage, cv::Point(HorizontalLine._p1.x, HorizontalLine._p1.y), cv::Point(HorizontalLine._p2.x, HorizontalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    resize(srcImage, srcImage, Size(1024, 618));
    cv::Point2f p1 = computeIntersect(HorizontalLine, VerticalLine);
    cv::Point2f p2, p3, p4;
    p2 = cv::Point2f(p1.x, p1.y + 1500);
    p3 = cv::Point2f(p1.x + 600, p2.y);
    Line strghtLine = Line(p2, p3);
    p4 = computeIntersect(strghtLine, VerticalLine);
    if (p4.x > p2.x)
    {
        double tan_theta = (double)(p4.x - p2.x) / 1500;
        turnAngle = std::atan(tan_theta) * 180 / PI;
    }
    else if (p4.x < p2.x)
    {
        double tan_theta = (double)(p2.x - p4.x) / 1500;
        turnAngle = -std::atan(tan_theta) * 180 / PI;
    }
    else
        turnAngle = 0;
    cornerPosition = cv::Point(-p1.x + Image_Width / 2, p1.y - Image_Height / 2);
    std::cout << turnAngle << std::endl;
    std::cout << cornerPosition << std::endl;
    if(abs(cornerPosition.x)<1297 && abs(cornerPosition.y) < 1025)
        return true;
    else return false;



    
    
    
    
    
    return true;
}
